/*
  底盘控制任务

*/
/* Includes ----------------------------------------------------------------- */

#include "Chassis.h"
#include "user_task.h"
#include "can_use.h"

static CAN_t can;
#ifdef DEBUG

CAN_Output_t out;

Chassis_t chassis;

CMD_t ctrl;

#else 

static CAN_Output_t out;

static Chassis_t chassis;

static Chassis_Ctrl_t ctrl;


#endif


PolarCoordinate_t wheelPolar[4];

PolarCoordinate_t wheelStatus[4];

/**
 * \brief 底盘任务
 *
 * \param argument 未使甄1�7
 */
void Task_Chassis(void *argument)
{
	
	 (void)argument; /* 未使用argument，消除警呄1�7 */
	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CHASSIS;

	Chassis_init(&chassis,&(task_runtime.config->chassis));

  uint32_t tick = osKernelGetTickCount(); 
	
  osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed5065, &can, NULL,
                    osWaitForever); 
  osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed6020, &can, NULL,
                    osWaitForever);
	while(1)
		{
#ifdef DEBUG
    task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
#endif
			//暂时没发现不同任务同时获取imu数据会出现问题
	osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed6020, &can, NULL, 0);
			
	osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed5065, &can, NULL, 0);
			
  osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
			
	osKernelLock();
			
    Chassis_UpdateFeedback(&chassis, &can);

	  Chassis_Control(&chassis,&ctrl,wheelStatus,&out);

    osKernelUnlock();

    osMessageQueueReset(task_runtime.msgq.can.output.chassis6020);
    osMessageQueuePut(task_runtime.msgq.can.output.chassis6020, &out.chassis6020, 0, 0);


    osMessageQueueReset(task_runtime.msgq.can.output.chassis5065);
    osMessageQueuePut(task_runtime.msgq.can.output.chassis5065, &out.chassis5065, 0, 0);
     
		tick += delay_tick;
    osDelayUntil(tick); 
		 
	  }
	
}
